//
// Created by ZhaoXiaoFei on 2022/8/5.
//

#include "key_frame.hpp"

KeyFrame::KeyFrame(double time_stamp_, int index_, Eigen::Vector3d& vio_T_w_i_, Eigen::Matrix3d& vio_R_w_i_, cv::Mat& image_,
                   std::vector<cv::Point3f>& point_3d_, std::vector<cv::Point2f>& point_2d_uv_, std::vector<cv::Point2f>& point_2d_xy_,
                   std::vector<double>& point_id_, int sequence_){
    time_stamp = time_stamp_;
    index = index_;
    vio_T_w_i = vio_T_w_i_;
    vio_R_w_i = vio_R_w_i_;
    T_w_i = vio_T_w_i;
    R_w_i = vio_R_w_i;
    origin_vio_T = vio_T_w_i;
    origin_vio_R = vio_R_w_i;

    image = image_.clone();

    point_3d = point_3d_;
    point_2d_uv = point_2d_uv_;
    point_2d_xy = point_2d_xy_;
    point_id = point_id_;
    sequence = sequence_;
    has_loop = false;
    loop_index = -1;
    loop_info = Eigen::Matrix<double, 8, 1>::Zero();
//    LOG(INFO) << "11111111111111" <<  std::endl;
    computeWindowBRIEFPoint();
//    LOG(INFO) << "11111111111111" <<  std::endl;
    computeBriefPoint();
}

void KeyFrame::computeWindowBRIEFPoint() {
    BriefExtractor briefExtractor(BRIEF_PATTERN_FILE);
    for(int i = 0; i < point_2d_uv.size(); i++){
        cv::KeyPoint key;
        key.pt = point_2d_uv[i];
        windowKeyPoints.push_back(key);
    }
    briefExtractor(image, windowKeyPoints, windowBriefDescriptors);
}

void KeyFrame::computeBriefPoint() {
    BriefExtractor briefExtractor(BRIEF_PATTERN_FILE);
    cv::FAST(image, keyPoints, 20, true);
    briefExtractor(image, keyPoints, briefDescriptors);
    for(int i = 0; i < keyPoints.size(); i++){
        Eigen::Vector3d temp;
        m_camera->liftProjective(Eigen::Vector2d(keyPoints[i].pt.x, keyPoints[i].pt.y), temp);
        cv::KeyPoint tmpK;
        tmpK.pt = cv::Point2f(temp.x() / temp.z(), temp.y() / temp.z());
        keyPoints_xy.push_back(tmpK);
    }
}

bool KeyFrame::searchInWindowByBF(const DVision::BRIEF::bitset window_descriptor,
                                  const std::vector<DVision::BRIEF::bitset>& briefDescriptors_old,
                                  const std::vector<cv::KeyPoint>& keyPoints_old,
                                  const std::vector<cv::KeyPoint>& keyPoints_xy_old,
                                  cv::Point2f &bestMatchPointUV,
                                  cv::Point2f &bestMatchPointXY){
    int minDistance = 128;
    int min_index = -1;
    for(int i = 0 ; i < briefDescriptors_old.size(); i++){
        int dis = HammingDistance(window_descriptor, briefDescriptors_old[i]);
        if(dis < minDistance){
            minDistance = dis;
            min_index = i;
        }
    }
    if(min_index != -1 && minDistance < 80){
        bestMatchPointUV = keyPoints_old[min_index].pt;
        bestMatchPointXY = keyPoints_xy_old[min_index].pt;
        return true;
    }else{
        return false;
    }
}

void KeyFrame::searchByBriefDistance(const std::vector<DVision::BRIEF::bitset> &briefDescriptors_old,
                                     const std::vector<cv::KeyPoint> &keyPoints_old,
                                     const std::vector<cv::KeyPoint> &keyPoints_xy_old,
                                     std::vector<cv::Point2f> &matched_point2d_old,
                                     std::vector<cv::Point2f> &matched_point2dXY_old, std::vector<bool> &status) {

    for(int i = 0; i < windowBriefDescriptors.size(); i++){
        cv::Point2f bestMatchPointUV(0.0f, 0.0f);
        cv::Point2f bestMatchPointXY(0.0f, 0.0f);
        if(searchInWindowByBF(windowBriefDescriptors[i], briefDescriptors_old, keyPoints_old, keyPoints_xy_old, bestMatchPointUV, bestMatchPointXY)){
            status.push_back(true);
        }
        else{
            status.push_back(false);
        }
        matched_point2d_old.push_back(bestMatchPointUV);
        matched_point2dXY_old.push_back(bestMatchPointXY);
    }
}

void KeyFrame::PnPByRansac(const std::vector<cv::Point2f> &matched_point2dXY_old,
                           const std::vector<cv::Point3f> &matched_point3d_cur,
                           Eigen::Matrix3d &R_old,
                           Eigen::Vector3d &T_old,
                           std::vector<bool>& status) {
    cv::Mat D, rvec, tvec, inliers, temp_r;
    Eigen::Matrix3d R_w_c = origin_vio_R * qic;
    Eigen::Vector3d T_w_c = origin_vio_T + origin_vio_R * tic;
    Eigen::Matrix3d R_initial = R_w_c.transpose();
    Eigen::Vector3d T_initial = -R_w_c.transpose() * T_w_c;

    cv::eigen2cv(R_initial, temp_r);
    cv::Rodrigues(temp_r, rvec);
    cv::eigen2cv(T_initial, tvec);

    cv::Mat K = cv::Mat::eye(3,3,CV_64F);
    cv::solvePnPRansac(matched_point3d_cur, matched_point2dXY_old, K, D,
                       rvec, tvec, true, 100, 10.0 / 460.0, 0.99, inliers);
    for(int i = 0; i < matched_point2dXY_old.size(); i++){
        status.push_back(0);
    }
//    LOG(INFO) << "inliers: " << inliers.rows << " " << inliers.cols << std::endl;
    for(int i = 0; i < inliers.rows; i++){
        status[inliers.at<int>(i)] = 1;
    }
    cv::Rodrigues(rvec, temp_r);
    Eigen::Matrix3d R_pnp, R_w_c_old;
    cv::cv2eigen(temp_r, R_pnp);
    R_w_c_old = R_pnp.transpose();
    Eigen::Vector3d T_pnp, T_w_c_old;
    cv::cv2eigen(tvec, T_pnp);
    T_w_c_old = -R_pnp.transpose() * T_pnp;
//    LOG(INFO) << "111111111111111 " << std::endl;

    R_old = R_w_c_old * qic.transpose();
    T_old = T_w_c_old - R_old * tic;
}

bool KeyFrame::findConnection(KeyFrame *old_kf) {
    std::vector<cv::Point2f> matched_point2d_cur{point_2d_uv};
    std::vector<cv::Point2f> matched_point2dXY_cur{point_2d_xy};
    std::vector<cv::Point3f> matched_3d{point_3d};
    std::vector<double> matched_id{point_id};

    std::vector<cv::Point2f> matched_point2d_old;
    std::vector<cv::Point2f> matched_point2dXY_old;
    std::vector<bool> status;
    searchByBriefDistance(old_kf->briefDescriptors, old_kf->keyPoints, old_kf->keyPoints_xy,
                          matched_point2d_old, matched_point2dXY_old, status);
    reduceVector(matched_point2d_old, status);
    reduceVector(matched_point2dXY_old, status);

    reduceVector(matched_point2d_cur, status);
    reduceVector(matched_point2dXY_cur, status);
    reduceVector(matched_3d, status);
    reduceVector(matched_id, status);
    status.clear();

    Eigen::Matrix3d R_old;
    Eigen::Vector3d T_old;

    if(matched_point2d_cur.size() > 25){
        status.clear();
        PnPByRansac(matched_point2dXY_old, matched_3d, R_old, T_old, status);
        reduceVector(matched_point2d_old, status);
        reduceVector(matched_point2dXY_old, status);

        reduceVector(matched_point2d_cur, status);
        reduceVector(matched_point2dXY_cur, status);
        reduceVector(matched_3d, status);
        reduceVector(matched_id, status);
    }
    Eigen::Matrix3d relative_R;
    Eigen::Vector3d relative_T;
    double relative_yaw;
    if(matched_point2d_cur.size() > 25){
        relative_R = R_old.transpose() * origin_vio_R;
        relative_T = R_old.transpose() * (origin_vio_T - T_old);
        relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(R_old).x());


        if(std::abs(relative_yaw) < 30.0 && relative_T.norm() < 20.0){
//            LOG(INFO) << "111111111111111 " << std::endl;
            has_loop = true;
            loop_index = old_kf->index;
            Eigen::Quaterniond temp(relative_R);
            loop_info << relative_T.x(), relative_T.y(), relative_T.z(),
                                temp.x(), temp.y(), temp.z(), temp.w(), relative_yaw;
            if(FAST_RELOCALIZATION){
                sensor_msgs::PointCloud msg_match_points;
                msg_match_points.header.stamp = ros::Time(time_stamp);
                for(int i = 0; i < matched_point2dXY_old.size(); i++){
                    geometry_msgs::Point32 p;
                    p.x = matched_point2dXY_old[i].x;
                    p.y = matched_point2dXY_old[i].y;
                    p.z = matched_id[i];
                    msg_match_points.points.push_back(p);
                }
                Eigen::Vector3d T = old_kf->T_w_i;
                Eigen::Matrix3d R = old_kf->R_w_i;
                Eigen::Quaterniond tempQ(R);
                sensor_msgs::ChannelFloat32 t_q_index;
                t_q_index.values.push_back(T.x());
                t_q_index.values.push_back(T.y());
                t_q_index.values.push_back(T.z());
                t_q_index.values.push_back(tempQ.w());
                t_q_index.values.push_back(tempQ.x());
                t_q_index.values.push_back(tempQ.y());
                t_q_index.values.push_back(tempQ.z());
                t_q_index.values.push_back(index);
                msg_match_points.channels.push_back(t_q_index);
                pub_match_points.publish(msg_match_points);
            }
            return true;
        }
    }
    return false;
}